#!/usr/bin/env python
import rospy, os
from std_srvs.srv import *
from std_msgs.msg import String



def handle_GoInP1(req):
    print 'Go in P1: TODO'
    return EmptyResponse()
    
def handle_GoInP2(req):
    print 'Go in P2: TODO'
    return EmptyResponse()
    
def handle_GoHome1(req):
    print 'Go Home1: TODO'
    return EmptyResponse()
    
def handle_GoHome2(req):
    print 'Go Home2: TODO'
    return EmptyResponse()
    
def handle_ExecCommand(req):
    print 'execute command: TODO'
    return EmptyResponse()

def speech (command):
    if command.data == 'go home':
        print 'ricevuto comando: Go Home'
        rospy.wait_for_service('/environment/HomeCommand')
        try:
            s = rospy.ServiceProxy('/environment/HomeCommand', Empty)
            s()
        except rospy.ServiceException, e:
            print "error service /environment/HomeCommand"
    if command.data == 'chiama':
        print 'ricevuto comando: chiama'
        rospy.wait_for_service('/environment/Command')
        try:
            s = rospy.ServiceProxy('/environment/Command', Empty)
            s()
        except rospy.ServiceException, e:
            print "error service /environment/Command"

if __name__ == '__main__':
    os.system("clear")
    print 'Starting manage_magabot'
    rospy.init_node('manage_magabot')
	
    rospy.Subscriber("recognizer/output", String, speech)
    rospy.Service('/magabot/GoInP1', Empty, handle_GoInP1)
    rospy.Service('/magabot/GoInP2', Empty, handle_GoInP2)
    rospy.Service('/magabot/ExecCommand', Empty, handle_ExecCommand)
    rospy.Service('/magabot/GoHome1', Empty, handle_GoHome1)
    rospy.Service('/magabot/GoHome2', Empty, handle_GoHome2)
    
    rospy.spin()
